#include "NIOperate.h"
const XnPoint3D NIOperate::ZeroXnPoint3D = {0,0,0};

// callback function of user generator: new user
void XN_CALLBACK_TYPE NewUser( xn::UserGenerator& generator,
	XnUserID user,
	void* pCookie )
{
	printf("OpenNI----New user identified: %d----\n",user);
	generator.GetSkeletonCap().RequestCalibration( user, true );
	//generator.GetPoseDetectionCap().StartPoseDetection("Psi", user);
}
void XN_CALLBACK_TYPE LostUser(xn::UserGenerator& generator,
	XnUserID user,
	void* pCookie )
{
	printf("Lost user %d\n", user);	
}

// callback function of skeleton: calibration end 
void XN_CALLBACK_TYPE CalibrationEnd( xn::SkeletonCapability& skeleton,
	XnUserID user,
	XnCalibrationStatus eStatus,
	void* pCookie )
{
	printf("OpenNI----Calibration complete for user----\n");
	if( eStatus == XN_CALIBRATION_STATUS_OK )
	{
		printf("OpenNI----Success----\n");
		skeleton.StartTracking( user );
	}
	else
	{
		printf("OpenNI----Failure----\n");
		skeleton.RequestCalibration( user, true );
		skeleton.StopTracking( user );
	}
}

NIOperate::NIOperate():m_SC(0),m_IsTracking(0),m_JointManager(NIJointManager(m_SC))//,ZeroXnPoint3D(xnCreatePoint3D(0,0,0))
{
	m_PrevJoint[0] = -1;
	m_PrevJoint[1] = 0;
	m_PrevJoint[2] = 1;
	m_PrevJoint[3] = 1;
	m_PrevJoint[4] = 3;
	m_PrevJoint[5] = 4;
	m_PrevJoint[6] = 1;
	m_PrevJoint[7] = 6;
	m_PrevJoint[8] = 7;
	m_PrevJoint[9] = 2;
	m_PrevJoint[10] = 9;
	m_PrevJoint[11] = 10;
	m_PrevJoint[12] = 2;
	m_PrevJoint[13] = 12;
	m_PrevJoint[14] = 13;
}
NIOperate::~NIOperate()
{
	m_SC.Release();
	delete m_hUserCB;
	delete m_hCalibCB;
	m_cBGRImg.release();
	m_c8BitDepth.release();
	delete [] m_Joints_3D;
	delete [] m_Joints_2D;
	delete [] m_PrevJoint;
}

bool NIOperate::InitContent()
{
	XnStatus eResult;
	// Register callback functions of user generator
	eResult = m_UserGenerator.RegisterUserCallbacks( NewUser, LostUser, NULL, m_hUserCB );
	if( !CheckOpenNIError( eResult, "RegisterUserCallbacks" ) )
	{
		return false;
	}
	// Register callback functions of skeleton from user generator
	m_SC = m_UserGenerator.GetSkeletonCap();
	m_SC.SetSkeletonProfile( XN_SKEL_PROFILE_ALL );
	eResult = m_SC.RegisterToCalibrationComplete( CalibrationEnd, &m_UserGenerator, m_hCalibCB );
	if( !CheckOpenNIError( eResult, "RegisterToCalibrationComplete" ) )
	{
		return false;
	}
	// Start generate
	eResult = m_Context.StartGeneratingAll();
	if( !CheckOpenNIError( eResult, "Start generate" ) )
	{
		return false;
	}
	return true;
}

void NIOperate::UpdateContent()
{
	// Convert to OpenCV form
	cv::Mat cColorImg = cv::Mat( m_ColorData.FullYRes(), m_ColorData.FullXRes(),
		CV_8UC3, (void*)m_ColorData.Data() );
	// Convert from RGB to BGR
	cv::cvtColor( cColorImg, m_cBGRImg, CV_RGB2BGR );
	// Convert to OpenCV form
	cv::Mat cDepthImg( m_DepthData.FullYRes(), m_DepthData.FullXRes(),
		CV_16UC1, (void*)m_DepthData.Data() );
	// Convert to 8 bit
	cDepthImg.convertTo( m_c8BitDepth, CV_8U, 255.0 / 7000 );

	UpdateUsers();
}

inline XnPoint3D NIOperate::GetSkeletonPos(XnSkeletonJoint jointName)
{
	XnSkeletonJointTransformation jointTran;
	m_SC.GetSkeletonJoint( m_UserID, jointName, jointTran);
	if(jointTran.position.fConfidence!=1)
	{
		//do something , like return NULL or print error
		return ZeroXnPoint3D;
	}
	return xnCreatePoint3D(jointTran.position.position.X , jointTran.position.position.Y , jointTran.position.position.Z);
}

inline void NIOperate::UpdateSkeleton()
{
	m_JointManager.Update(m_UserID);

	for(int i=0;i<15;i++)
	{
		m_Joints_3D[i] = m_JointManager[i].GetCurPoint();
	}

	m_DepthGenerator.ConvertRealWorldToProjective(15 , m_Joints_3D , m_Joints_2D);

	

	
	Database* data;
	data = Database::getInstance();
	data->NIJoint2D = m_Joints_2D;
	data->NIJoint3D = m_Joints_3D;
}

inline void NIOperate::UpdateUsers()
{
	if( m_nUsers > 0 )
	{
		// 11. get users 
		//XnUserID* aUserID = new XnUserID[m_nUsers];
		XnUserID aUserID[15];
		XnUInt16 nUsers = 15;
		m_UserGenerator.GetUsers( aUserID, nUsers );
		// 12. check each user
		for( int i = 0; i < nUsers; ++i )
		{
			// 13. if is tracking skeleton
			if( m_SC.IsTracking( aUserID[i] ) )
			{
				m_IsTracking = true;
				m_UserID = aUserID[i];
				// 14. get skeleton joint data
				UpdateSkeleton();
				//printf("Is tracking : %d\n",aUserID[i]);
			}
			else
			{
				m_IsTracking = false;
				m_SC.StopTracking( aUserID[i] );
			}
		}
	}
	else
	{
		m_IsTracking = false;
		m_SC.StopTracking( m_UserID );
		m_UserID = 0;
	}
}